#include <ros/ros.h>
#include <map_study/map_pub.h>

static const unsigned char NO_INFORMATION = 255;
static const unsigned char LETHAL_OBSTACLE = 254;
static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
static const unsigned char FREE_SPACE = 0;

template<typename data_type>
void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y,
                   unsigned int sm_size_x, data_type* dest_map, unsigned int dm_lower_left_x,
                   unsigned int dm_lower_left_y, unsigned int dm_size_x, unsigned int region_size_x,
                   unsigned int region_size_y)
{
  // we'll first need to compute the starting points for each map
  data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
  data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);

  cout << "[copyMapRegion] sm_index: " << sm_lower_left_y * sm_size_x + sm_lower_left_x << " dm_index: " << dm_lower_left_y * dm_size_x + dm_lower_left_x << endl;

  // now, we'll copy the source map into the destination map
  for (unsigned int i = 0; i < region_size_y; ++i)
  {
    memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
    sm_index += sm_size_x;
    dm_index += dm_size_x;
  }
}


unsigned char interpretValue(unsigned char value)
{
    bool track_unknown_space_ = true;
    int temp_lethal_threshold = 100;
    int temp_unknown_cost_value = -1;
    bool trinary_costmap_ = true;
    // int temp_lethal_threshold, temp_unknown_cost_value;
    // nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
    // nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
    // nh.param("trinary_costmap", trinary_costmap_, true);
    unsigned char lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
    unsigned char unknown_cost_value_ = temp_unknown_cost_value;

  // check if the static value is above the unknown or lethal thresholds
  if (track_unknown_space_ && value == unknown_cost_value_)
    return NO_INFORMATION;
  else if (!track_unknown_space_ && value == unknown_cost_value_)
    return FREE_SPACE;
  else if (value >= lethal_threshold_)
    return LETHAL_OBSTACLE;
  else if (trinary_costmap_)
    return FREE_SPACE;

  double scale = (double) value / lethal_threshold_;
  return scale * LETHAL_OBSTACLE;
}

class MapColor{
    public:
        MapColor();
        ~MapColor() = default;

    private:
        std::shared_ptr<MapPublish> map_pub_ptr_;
        std::shared_ptr<MapPublish> map_pub_update_ptr_;
};



MapColor::MapColor(){
    map_pub_ptr_ = std::make_shared<MapPublish>("/map_origin");
    map_pub_update_ptr_ = std::make_shared<MapPublish>("/map_update_origin");

    static char* cost_translation_table_ = new char[256];
    // special values:
    cost_translation_table_[0] = 0;  // NO obstacle
    cost_translation_table_[253] = 99;  // INSCRIBED obstacle
    cost_translation_table_[254] = 100;  // LETHAL obstacle
    cost_translation_table_[255] = -1;  // UNKNOWN
    // regular cost values scale the range 1 to 252 (inclusive) to fit
    // into 1 to 98 (inclusive).
    for (int i = 1; i < 253; i++)
    {
      cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251);
    }

    nav_msgs::OccupancyGrid map_msg;
    map_msg.info.resolution = 0.05;
    map_msg.info.width = 256;
    map_msg.info.height = 200;
    map_msg.info.origin.position.x = -5.0;
    map_msg.info.origin.position.y = -1.0;
    map_msg.info.origin.position.z = 0.0;
    map_msg.info.origin.orientation.w = 1.0;
    map_msg.header.frame_id = "map";
    map_msg.header.stamp = ros::Time::now();

    for(int j = 0; j < map_msg.info.height; j++){
        for(int i = 0; i < map_msg.info.width; i++){
            int8_t data = i > 100 ? (i > 200 ? -1 : 0) : 100;
            map_msg.data.push_back(data);
        }
    }

    // 转换字段数据内容
    float new_origin_x = -8.03;
    float new_origin_y = -2.07;
    int size_x_ = map_msg.info.width;
    int size_y_ = map_msg.info.height;
    float origin_x_ = map_msg.info.origin.position.x;
    float origin_y_ = map_msg.info.origin.position.y;
    float resolution_ = map_msg.info.resolution;
    unsigned char default_value_ = NO_INFORMATION;
    unsigned char* costmap_;
    costmap_ = new unsigned char[size_x_ * size_y_];
    unsigned int index = 0;
    // initialize the costmap with static data
    for (unsigned int i = 0; i < map_msg.info.height; ++i){
      for (unsigned int j = 0; j < map_msg.info.width; ++j){
        unsigned char value = map_msg.data[index];
        costmap_[index] = interpretValue(value);
        ++index;
      }
    }


    index = 0;
    map_msg.data.resize(size_x_ * size_y_);
    for(int j = 0; j < map_msg.info.height; j++){
        for(int i = 0; i < map_msg.info.width; i++){
            map_msg.data[index] = cost_translation_table_[costmap_[index]];
            ++index;
        }
    }
    map_pub_ptr_->publishMap(map_msg);


    // 更新原点
    int cell_ox, cell_oy;
    cell_ox = int((new_origin_x - origin_x_) / resolution_);
    cell_oy = int((new_origin_y - origin_y_) / resolution_);
    cout << "cell_ox: " << cell_ox << ", cell_oy: " << cell_oy << endl;
  
    // Nothing to update
    if (cell_ox == 0 && cell_oy == 0)
      return;
  
    // compute the associated world coordinates for the origin cell
    // because we want to keep things grid-aligned
    double new_grid_ox, new_grid_oy;
    new_grid_ox = origin_x_ + cell_ox * resolution_;
    new_grid_oy = origin_y_ + cell_oy * resolution_;
    cout << "new_grid_ox: " << new_grid_ox << ", new_grid_oy: " << new_grid_oy << endl;

    // To save casting from unsigned int to int a bunch of times
    int size_x = size_x_;
    int size_y = size_y_;
    cout << "size_x: " << size_x << ", size_y: " << size_y << endl;
  
    // we need to compute the overlap of the new and existing windows
    int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
    lower_left_x = min(max(cell_ox, 0), size_x);
    lower_left_y = min(max(cell_oy, 0), size_y);
    upper_right_x = min(max(cell_ox + size_x, 0), size_x);
    upper_right_y = min(max(cell_oy + size_y, 0), size_y);
  
    unsigned int cell_size_x = upper_right_x - lower_left_x;
    unsigned int cell_size_y = upper_right_y - lower_left_y;

    cout << "lower_left_x: " << lower_left_x << ", upper_right_x: " << upper_right_x 
        << ", lower_left_y: " << lower_left_y << ", upper_right_y: " << upper_right_y 
        << ", cell_size_x: " << cell_size_x << ", cell_size_y: " << cell_size_y 
        << endl;
  
    // we need a map to store the obstacles in the window temporarily
    unsigned char* local_map = new unsigned char[cell_size_x * cell_size_y];
  
    // copy the local window in the costmap to the local map
    copyMapRegion(costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, cell_size_y);
  
    // now we'll set the costmap to be completely unknown if we track unknown space
    // resetMaps();
    memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
  
    // update the origin with the appropriate world coordinates
    origin_x_ = new_grid_ox;
    origin_y_ = new_grid_oy;
    cout << "origin_x_: " << origin_x_ << ", origin_y_: " << origin_y_ << endl;

    // compute the starting cell location for copying data back in
    int start_x = lower_left_x - cell_ox;
    int start_y = lower_left_y - cell_oy;

    cout << "start_x: " << start_x << ", start_y: " << start_y << endl;
    // now we want to copy the overlapping information back into the map, but in its new location
    copyMapRegion(local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y);
  
    // make sure to clean up
    delete[] local_map;







    map_msg.info.origin.position.x = origin_x_;
    map_msg.info.origin.position.y = origin_y_;
    index = 0;
    map_msg.data.resize(size_x_ * size_y_);
    for(int j = 0; j < map_msg.info.height; j++){
        for(int i = 0; i < map_msg.info.width; i++){
            map_msg.data[index] = cost_translation_table_[costmap_[index]];
            ++index;
        }
    }
    map_pub_update_ptr_->publishMap(map_msg);
}



int main(int argc, char** argv){
    ros::init(argc, argv, "map_color_node");
    MapColor map_color;
    ros::spin();
    return 0;
}